/*
* Copyright (c) 2006-2007 Erin Catto http:
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked, and must not be
* misrepresented the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/

// kevmoo 2010-07-30 - Nothing seems to use this...hmmm
goog.provide('box2d.Conservative');

/**
 @constructor
 */
box2d.Conservative = function() {};

box2d.Conservative.R1 = new box2d.Mat22();
box2d.Conservative.R2 = new box2d.Mat22();
box2d.Conservative.x1 = new box2d.Vec2();
box2d.Conservative.x2 = new box2d.Vec2();
box2d.Conservative.Conservative = function(shape1, shape2) {
  var body1 = shape1.GetBody();
  var body2 = shape2.GetBody();

  //box2d.Vec2 v1 = body1->m_position - body1->m_position0;
  var v1X = body1.m_position.x - body1.m_position0.x;
  var v1Y = body1.m_position.y - body1.m_position0.y;
  //float32 omega1 = body1->m_rotation - body1->m_rotation0;
  var omega1 = body1.m_rotation - body1.m_rotation0;
  //box2d.Vec2 v2 = body2->m_position - body2->m_position0;
  var v2X = body2.m_position.x - body2.m_position0.x;
  var v2Y = body2.m_position.y - body2.m_position0.y;
  //float32 omega2 = body2->m_rotation - body2->m_rotation0;
  var omega2 = body2.m_rotation - body2.m_rotation0;

  //float32 r1 = shape1->GetMaxRadius();
  var r1 = shape1.GetMaxRadius();
  //float32 r2 = shape2->GetMaxRadius();
  var r2 = shape2.GetMaxRadius();

  //box2d.Vec2 p1Start = body1->m_position0;
  var p1StartX = body1.m_position0.x;
  var p1StartY = body1.m_position0.y;
  //float32 a1Start = body1->m_rotation0;
  var a1Start = body1.m_rotation0;

  //box2d.Vec2 p2Start = body2->m_position0;
  var p2StartX = body2.m_position0.x;
  var p2StartY = body2.m_position0.y;
  //float32 a2Start = body2->m_rotation0;
  var a2Start = body2.m_rotation0;

  //box2d.Vec2 p1 = p1Start;
  var p1X = p1StartX;
  var p1Y = p1StartY;
  //float32 a1 = a1Start;
  var a1 = a1Start;
  //box2d.Vec2 p2 = p2Start;
  var p2X = p2StartX;
  var p2Y = p2StartY;
  //float32 a2 = a2Start;
  var a2 = a2Start;

  //b2Mat22 box2d.Conservative.R1(a1), box2d.Conservative.R2(a2);
  box2d.Conservative.R1.Set(a1);
  box2d.Conservative.R2.Set(a2);

  //shape1->QuickSync(p1, box2d.Conservative.R1);
  var p1 = new box2d.Vec2(p1X, p1Y);
  shape1.QuickSync(p1, box2d.Conservative.R1);
  //shape2->QuickSync(p2, box2d.Conservative.R2);
  var p2 = new box2d.Vec2(p2X, p2Y);
  shape2.QuickSync(p2, box2d.Conservative.R2);

  //float32 s1 = 0.0f;
  var s1 = 0.0;
  //const int32 maxIterations = 10;
  var maxIterations = 10;
  //box2d.Vec2 d;
  var dX;
  var dY;
  //float32 invRelativeVelocity = 0.0f;
  var invRelativeVelocity = 0.0;
  //bool hit = true;
  var hit = true;
  //box2d.Vec2 box2d.Conservative.x1, box2d.Conservative.x2; moved to static var
  for (var iter = 0; iter < maxIterations; ++iter) {
    // Get the accurate distance between shapes.
    //float32 distance = box2d.Distance.Distance(&box2d.Conservative.x1, &box2d.Conservative.x2, shape1, shape2);
    var distance = box2d.Distance.Distance(box2d.Conservative.x1, box2d.Conservative.x2, shape1, shape2);
    if (distance < box2d.Settings.b2_linearSlop) {
      if (iter == 0) {
        hit = false;
      } else {
        hit = true;
      }
      break;
    }

    if (iter == 0) {
      //box2d.Vec2 d = box2d.Conservative.x2 - box2d.Conservative.x1;
      dX = box2d.Conservative.x2.x - box2d.Conservative.x1.x;
      dY = box2d.Conservative.x2.y - box2d.Conservative.x1.y;
      //d.Normalize();
      var dLen = Math.sqrt(dX * dX + dY * dY);
      //float32 relativeVelocity = b2Dot(d, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
      var relativeVelocity = (dX * (v1X - v2X) + dY * (v1Y - v2Y)) + Math.abs(omega1) * r1 + Math.abs(omega2) * r2;
      if (Math.abs(relativeVelocity) < Number.MIN_VALUE) {
        hit = false;
        break;
      }

      invRelativeVelocity = 1.0 / relativeVelocity;
    }

    // Get the conservative movement.
    //float32 ds = distance * invRelativeVelocity;
    var ds = distance * invRelativeVelocity;
    //float32 s2 = s1 + ds;
    var s2 = s1 + ds;

    if (s2 < 0.0 || 1.0 < s2) {
      hit = false;
      break;
    }

    if (s2 < (1.0 + 100.0 * Number.MIN_VALUE) * s1) {
      hit = true;
      break;
    }

    s1 = s2;

    // Move forward conservatively.
    //p1 = p1Start + s1 * v1;
    p1X = p1StartX + s1 * v1X;
    p1Y = p1StartY + s1 * v1Y;
    //a1 = a1Start + s1 * omega1;
    a1 = a1Start + s1 * omega1;
    //p2 = p2Start + s1 * v2;
    p2X = p2StartX + s1 * v2X;
    p2Y = p2StartY + s1 * v2Y;
    //a2 = a2Start + s1 * omega2;
    a2 = a2Start + s1 * omega2;

    box2d.Conservative.R1.Set(a1);
    box2d.Conservative.R2.Set(a2);
    shape1.QuickSync(p1, box2d.Conservative.R1);
    shape2.QuickSync(p2, box2d.Conservative.R2);
  }

  if (hit) {
    // Hit, move bodies to safe position and re-sync shapes.
    //box2d.Vec2 d = box2d.Conservative.x2 - box2d.Conservative.x1;
    dX = box2d.Conservative.x2.x - box2d.Conservative.x1.x;
    dY = box2d.Conservative.x2.y - box2d.Conservative.x1.y;
    //float32 length = d.Length();
    var length = Math.sqrt(dX * dX + dY * dY);
    if (length > box2d.Settings.FLT_EPSILON) {
      dX *= box2d.Settings.b2_linearSlop / length;
      dY *= box2d.Settings.b2_linearSlop / length;
    }

    if (body1.IsStatic()) {
      //body1.m_position = p1;
      body1.m_position.x = p1X;
      body1.m_position.y = p1Y;
    } else {
      //body1.m_position = p1 - d;
      body1.m_position.x = p1X - dX;
      body1.m_position.y = p1Y - dY;
    }
    body1.m_rotation = a1;
    body1.m_R.Set(a1);
    body1.QuickSyncShapes();

    if (body2.IsStatic()) {
      //body2->m_position = p2;
      body2.m_position.x = p2X;
      body2.m_position.y = p2Y;
    } else {
      //body2->m_position = p2 + d;
      body2.m_position.x = p2X + dX;
      body2.m_position.y = p2Y + dY;
    }
    //body2.m_position = p2 + d;
    body2.m_position.x = p2X + dX;
    body2.m_position.y = p2Y + dY;
    body2.m_rotation = a2;
    body2.m_R.Set(a2);
    body2.QuickSyncShapes();

    return true;
  }

  // No hit, restore shapes.
  shape1.QuickSync(body1.m_position, body1.m_R);
  shape2.QuickSync(body2.m_position, body2.m_R);
  return false;
};
